#include <gravity_well_control.h>

namespace controllers
{
	Gravity_Well_Control::Gravity_Well_Control() {}
	Gravity_Well_Control::~Gravity_Well_Control() {}

	bool Gravity_Well_Control::init(std::vector<HHD> &hHD)
	{
		_hHD = hHD;

		kStiffness	<< 0.075,0.075,0.075;
		kDamping	<< 0.001,0.001,0.001;
		kGravityWellInfluence = 40;

		wellPos		<<   0,  0,  0;
		wellVel		<<   0,  0,  0;

		return true;
	}

	void Gravity_Well_Control::starting(const boost::posix_time::ptime& time)
	{
		for (int i = 0; i < _hHD.size(); i++)
		{
			hdMakeCurrentDevice(_hHD[i]);

			/* Begin haptics frame.  ( In general, all state-related haptics calls
			should be made within a frame. ) */
			//hdBeginFrame(_hHD[i]);

			/* Get the current position of the device. */
			hdGetDoublev(HD_CURRENT_POSITION, hdu_position[i]);
			/* Get the current velocity of the device. */
			//hdGetDoublev(HD_CURRENT_VELOCITY, hdu_velocity[i]);

			for (int j = 0; j < 3; j++)
			{
				position[i](j) = hdu_position[i][j];
				//velocity[i](j) = hdu_velocity[i][j];
			}

			/* End haptics frame. */
			//hdEndFrame(_hHD[i]);
		}
		wellPos = position[0];
	}

	void Gravity_Well_Control::update(const boost::posix_time::ptime& time, const boost::posix_time::time_duration& period)
	{
		for (int i = 0; i < _hHD.size(); i++)
		{
			hdMakeCurrentDevice(_hHD[i]);

			/* Begin haptics frame.  ( In general, all state-related haptics calls
			should be made within a frame. ) */
			//hdBeginFrame(_hHD[i]);

			/* Get the current position of the device. */
			hdGetDoublev(HD_CURRENT_POSITION, hdu_position[i]);
			/* Get the current velocity of the device. */
			hdGetDoublev(HD_CURRENT_VELOCITY, hdu_velocity[i]);

			for (int j = 0; j < 3; j++)
			{
				position[i](j) = hdu_position[i][j];
				velocity[i](j) = hdu_velocity[i][j];
			}

			force[i].setZero();
			force_p[i].setZero();
			force_d[i].setZero();
		}

		memset(hdu_force, 0, 2*sizeof(hduVector3Dd));

		/* >  positionTwell = wellPos-position  < 
		Create a vector from the device position towards the gravity 
		well's center. */

		positionTwell[0] = wellPos - position[0];
		/* absolute damping */
		velocityTwell[0] = wellVel - velocity[0];

		/* If the device position is within some distance of the gravity well's 
		center, apply a spring force towards gravity well's center.  The force
		calculation differs from a traditional gravitational body in that the
		closer the device is to the center, the less force the well exerts;
		the device behaves as if a spring were connected between itself and
		the well's center. */
		for (int i = 0; i < _hHD.size(); i++)
		{
			//if (hduVecMagnitude(positionTwell[i]) < kGravityWellInfluence)
			if (positionTwell[i].norm() < kGravityWellInfluence)
			{
				/* >  F = k * x - d * v  < 
				F: Force in Newtons (N)
				k: Stiffness of the well (N/mm)
				x: Vector from the device endpoint position to the center 
				of the well. */
				force_p[i] = positionTwell[i].cwiseProduct(kStiffness);
				force_d[i] = velocityTwell[i].cwiseProduct(kDamping);
				force[i] = force_p[i] + force_d[i];
				for (int j = 0; j < 3; j++)
				{
					hdu_force[i][j] = force[i](j);
				}
			}
		}

		for (int i = 0; i < _hHD.size(); i++)
		{
			hdMakeCurrentDevice(_hHD[i]);

			/* Send the force to the device. */
			hdSetDoublev(HD_CURRENT_FORCE, hdu_force[i]);

			/* End haptics frame. */
			//hdEndFrame(_hHD[i]);

			/* Check for errors and abort the callback if a scheduler error
			is detected. */
			if (HD_DEVICE_ERROR(error = hdGetError()))
			{
				hduPrintError(stderr, &error, 
					"Error detected while rendering gravity well\n");

				//if (hduIsSchedulerError(&error))
				//{
				//	return HD_CALLBACK_DONE;
				//}
			}
		}

		/* Signify that the callback should continue running, i.e. that
		it will be called again the next scheduler tick. */
		//return HD_CALLBACK_CONTINUE;
	}

	void Gravity_Well_Control::command()
	{
		
	}

	void Gravity_Well_Control::stopping(const boost::posix_time::ptime& time)
	{
		
	}
}
